package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;
import java.util.Vector;

/**
 *
 * @author Fondy Fire
 */
public class RobotArmFF {
    
    private Joystick[] joyArray;

    private Vector buttonMap;

    private PWM armMotor = new PWM(5);
    

    // used to determin if the claw has been set this iteration
    private boolean clawSet;
    private boolean postSet;
    
    // claw lower and upper motors. 
    private Relay clawUpper; // Relay 4
    private Relay clawLower; // Relay 5
    
    // timer, used to debounce 
    private TimerFF timer;

    
    private Solenoid centerColumnUp;    // Solenoid 6
    private Solenoid centerColumnDown;  // Solenoid 7

    
    /** Magnetic switch on the arm. See armPosition */
    private DigitalInput armSensor;

    /**
     * Position 1 is floor
     * Position 2 is middle
     * Position 3 is top*/
    private int armPosition;
    /** -1 = down, 0 = not moving, 1 = moving up*/
    private int armDir;
    
    private boolean lastReading;
    private int lastDir;
    private double timeSinceChange;
    
    private double postRiseTime = 5000;
    private double postTime;
    private boolean postTop;
    private boolean readyForNext;



    /**
     * Inner abstract class with one method: actuate()
     * The idea is that the actuate method is called when the joy button
     * combo given at instalation is pressed.
     */
    private abstract class JoyButton{

        public int joy;
        public int button;
        public String action;

        public JoyButton(int j, int b, String a){
            joy = j;
            button = b;
            action = a;
        }

        public abstract void actuate();

    }

    /* ========== Constructor =================================*/

    /**
     * Constructor
     *
     * Standard Joy ordering:
     * 0 = nothing
     * 1 = left
     * 2 = right
     * 3 = coPilot
     * 
     * @param joys
     */
    public RobotArmFF(Joystick[] joys){

        armDir = 0;
        // it is assumed we start on the floor.
        armPosition = 0;

        timer = new TimerFF();

        centerColumnUp = new Solenoid(6);
        centerColumnDown = new Solenoid(7);

        clawUpper = new Relay(4);
        clawLower = new Relay(5);

        joyArray = joys;
        buttonMap = new Vector();

        armSensor = new DigitalInput(5);

        initButtonMap();

    }




    /** Maps joystick buttons to actions on the arm. */
    private void initButtonMap(){
        buttonMap.addElement( new JoyButton(3,5, "Post Up"){


            public void actuate() {
                centerColumnUp.set(true);
                centerColumnDown.set(false);
                postSet = true;
                
            }


        });

        buttonMap.addElement( new JoyButton(3,7, "Post Down"){
            public void actuate() {
                centerColumnUp.set(false);
                centerColumnDown.set(true);
                postSet = true;
                
            }});

         buttonMap.addElement( new JoyButton(3,6, "Pull Tube"){
            public void actuate() {
                clawUpper.set(Relay.Value.kReverse);
                clawLower.set(Relay.Value.kReverse);
                clawSet = true;
            }});

         buttonMap.addElement( new JoyButton(3,8, "Push Tube"){
            public void actuate() {
                clawUpper.set(Relay.Value.kForward);
                clawLower.set(Relay.Value.kForward);
                clawSet = true;
            }});

         buttonMap.addElement( new JoyButton(3,1, "Arm Floor"){
            public void actuate() {
                // Move the arm to the floor position
            }});

         buttonMap.addElement( new JoyButton(3,2, "Arm Bottom"){
            public void actuate() {
                // Move the arm to the bottom rack
            }});

         buttonMap.addElement( new JoyButton(3,3, "Arm Middle"){
            public void actuate() {
                // Move the arm to the middle row of pegs
            }});

         buttonMap.addElement( new JoyButton(3,4, "Arm Top"){
            public void actuate() {
                // Move the arm to the top row of pegs
            }});
    }

    

    /** Softens joysticks
     * @param x
     * @return signOf(x) * (x^2) */
    private double softStart(double x){ return ((x > 0)? 1 : -1)* (x * x); }

    public int getCMD(String name){
        for(int i = 0; i < buttonMap.size(); i++ ){
            JoyButton curr = (JoyButton) buttonMap.elementAt(i);
            if(curr.action.equals(name)){
                return i;
            }
        }
        return -1;
    }
    
    public void actuate(int CMD){
        if(CMD >= 0 && CMD < buttonMap.size()){
            ((JoyButton) buttonMap.elementAt(CMD)).actuate();
        }
    }
    
    public void update(){

        // not yet implemented
        //updatePos();
        
        clawSet = false;
        postSet = false;

        // take care of joy button mappings. 
        for(int i = 0; i < buttonMap.size(); i++ ){
            JoyButton curr = (JoyButton) buttonMap.elementAt(i);
            if(joyArray[curr.joy].getRawButton(curr.button)){
                curr.actuate();
            }
        }


        
        // take care of movement.
        armMotor.setRaw(128 + (int)(120 * joyArray[3].getY()));
        
        // Tube rotate code.
        if(((int)(120 * joyArray[3].getRawAxis(4))) < -20){
            clawUpper.set(Relay.Value.kForward);
            clawLower.set(Relay.Value.kReverse);
            clawSet = true;
        } else if (((int)(120 * joyArray[3].getRawAxis(4))) > 20){
            clawUpper.set(Relay.Value.kReverse);
            clawLower.set(Relay.Value.kForward);
            clawSet = true;
        }

        
        // Stop the claw if the copiolt takes his hand off the button
        if(!clawSet){
            clawUpper.set(Relay.Value.kOff);
            clawLower.set(Relay.Value.kOff);
        }

        // stop the post if the copiolt takes his hand off the button
        if(!postSet){
            centerColumnUp.set(false);
            centerColumnDown.set(false);
        }
        
    }

    /** Take care of determining the proper arm position. Call once per
     * tick. Not working.
     * Arm below at position 1, reading false
     * Position 1 is floor
     * Position 2 is middle
     * Position 3 is top
     * When the reading changes to false, wait 10 Milliseconds
     * 
     */
    private void updatePos(){
        boolean currReading = armSensor.get();
        if(armDir != 0){
            lastDir = armDir;
        }

        if(currReading != lastReading && currReading){
            armPosition += lastDir;
        }


    /**
     * Position 1 is floor
     * Position 2 is middle
     * Position 3 is top*/
    //armPosition;
    /** -1 = down, 0 = not moving, 1 = moving up*/
    //armDir;

    //lastReading;
    //lastDir;
    //timeSinceChange;
    }


    /*Get the arm position */
    public int getArmPosition(){
        return armPosition;
    }
    
    /** =========== Autonomous Code =========================================*/
    
    private boolean lastArmReadingAuto;
    
    /** 
     * Returns the best guess of the post's location. Works by reading the
     * length of time the post has been moving up, and comparing that to the 
     * average time it takes the post to move all the way up. Intended for
     * use in autonomous
     */
    public boolean isPostUp(){
        return postTime > (postRiseTime - 100);
    }

    public void updateAuto(){
        autoTimer.update();
    }
    
    private TimerFF autoTimer;
    
    public void initAuto(){
        autoTimer = new TimerFF();
        autoTimer.init();
        lastArmReadingAuto = false;
    }
    
    /**
     * method meant to be called from autonomous, raise post;
     *
     * This is an ugly hack, I'd rather not write duplicate code like this,
     * but time is running out.
     */
    public void postUpAuto(){
        centerColumnUp.set(true);
        centerColumnDown.set(false);

        postTime += autoTimer.frameMilli;
        if(postTime > postRiseTime ){
            postTime = postRiseTime;
        }
    }
    
    /**
     * Stops the post
     */
    public void postStopAuto(){
        centerColumnUp.set(false);
        centerColumnDown.set(false);
    }

    /**
     * method meant to be called from autonomous, lower post;
     *
     * This is an ugly hack, I'd rather not write duplicate code like this,
     * but time is running out.
     */
    public void postDownAuto(){
        centerColumnUp.set(false);
        centerColumnDown.set(true);

        postTime -= autoTimer.frameMilli;
        if(postTime < 0){
            postTime = 0;
        }
    }
    
    /**
     * time since last reading change.
     */
    private double armRaiseTimeAuto;
    
    /**
     * expected minimum time between readings, in milliseconds
     */
    private final double DEBOUNCETIME = 1000;
    
    /**
     * Raise the arm to the given position
     * pos Valid values:
     * 1 = floor
     * 2 = middle
     * 3 = top
     * 
     * Speed: 0.0 to 1.0
     * 
     * 'Nother ugly hack
     */
    public void armToPosAuto(int pos, double speed){
        if(pos < 0 || pos > 3){
            armMotor.setRaw(128);
            return;
        }

        int dir = 0;
        if(armPosition == pos){
            armMotor.setRaw(128);
            return;
        } else if (armPosition < pos) {
            dir = 1;
        } else if (armPosition > pos) {
            dir = -1;
        }

        boolean currReading = armSensor.get();

        armRaiseTimeAuto += autoTimer.frameMilli;

        // have to wait at least DEBOUNCETIME between readings to 
        // avoid false readings. (Sometimes called 'bounce')
        if(armRaiseTimeAuto > DEBOUNCETIME){
            if(!currReading){
                readyForNext = true;
                armRaiseTimeAuto = 0;
            } else if (readyForNext){
                changePosition(dir);
            }
        }

        armMotor.setRaw(128 - (dir) * (int)(120 * speed));
        
    }
    
    /**
     * Valid values 1, -1 for up and down, respectively
     * @param dir
     */
    private void changePosition(int dir){
        armPosition += dir;
        armRaiseTimeAuto = 0;
        //lastArmReadingAuto = true;

        // emergancy fail safe, if the debounce works correctly, we should not
        // need these.
        if(armPosition > 3){
            armPosition = 3;
            
        } else if(armPosition < 0){
            armPosition = 0;
        }
        
    }

    //========================== More Hacked code =============================
    private boolean armAtTop;

    public void armToTop(double speed){

        System.out.println(armSensor.get());

        if(armSensor.get()){
            armMotor.setRaw(128 - (int)(120 * speed));
            armAtTop = false;
        } else {
            armMotor.setRaw(128);
            armAtTop = true;
        }
    }

    public void tubePushOutAuto(){
        clawUpper.set(Relay.Value.kForward);
        clawLower.set(Relay.Value.kForward);
    }

    public void tubeRotateForwardAuto(){
        clawUpper.set(Relay.Value.kReverse);
        clawLower.set(Relay.Value.kForward);
    }

    public void stopArmAuto() {
        armMotor.setRaw(128);
    }

}
